/**
 * @file ebex_genetic_mating.c
 *
 * @date Oct 24, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <stddef.h>
#include <ebex_quaternion.h>

#include <ebex_genetic.h>
#include <ebex_genetic_mating.h>

static void ebex_genetic_mating_double_mixing(population_t *m_pop __attribute__((unused)), organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome);
static void ebex_genetic_mating_double_mean(population_t *m_pop __attribute__((unused)), organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome);
static void ebex_genetic_mating_double_allele_mixing(population_t *m_pop __attribute__((unused)), organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome);
static void singlepoint_mating_double_chromosome(population_t *m_pop __attribute__((unused)), double *m_dad, double *m_mom, double *m_son,
		double *m_daughter, size_t m_chromosomelen);
static void doublepoint_mating_double_chromosome(population_t *m_pop __attribute__((unused)), double *m_dad, double *m_mom, double *m_son,
		double *m_daughter, size_t m_chromosomelen);
static void ebex_genetic_mating_quaternion_mixing(population_t *m_pop __attribute__((unused)), organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome);
static void ebex_genetic_mating_quaternion_mean(population_t *m_pop __attribute__((unused)), organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome);
static void ebex_genetic_mating_quaternion_allele_mixing(population_t *m_pop __attribute__((unused)), organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome);
static void singlepoint_mating_quaternion_chromosome(population_t *m_pop __attribute__((unused)), ebex_quat_t *m_dad, ebex_quat_t *m_mom, ebex_quat_t *m_son,
		ebex_quat_t *m_daughter, size_t m_chromosomelen);
static void doublepoint_mating_quaternion_chromosome(population_t *m_pop __attribute__((unused)), ebex_quat_t *m_dad, ebex_quat_t *m_mom, ebex_quat_t *m_son,
		ebex_quat_t *m_daughter, size_t m_chromosomelen);


static void ebex_genetic_mating_double_mixing(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome)
{

	/* Checks */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed");

	if (ran_bool())
	{
		memcpy(m_son->chromosome[m_chromosome].dbl_allele, m_dad->chromosome[m_chromosome].dbl_allele, m_dad->chromosome[m_chromosome].length * sizeof(double));
		memcpy(m_daughter->chromosome[m_chromosome].dbl_allele, m_mom->chromosome[m_chromosome].dbl_allele, m_dad->chromosome[m_chromosome].length * sizeof(double));
		ebex_genetic_copy_data(m_pop, m_son, m_dad, m_chromosome);
		ebex_genetic_copy_data(m_pop, m_daughter, m_mom, m_chromosome);
	}
	else
	{
		memcpy(m_daughter->chromosome[m_chromosome].dbl_allele, m_dad->chromosome[m_chromosome].dbl_allele, m_dad->chromosome[m_chromosome].length * sizeof(double));
		memcpy(m_son->chromosome[m_chromosome].dbl_allele, m_mom->chromosome[m_chromosome].dbl_allele, m_dad->chromosome[m_chromosome].length * sizeof(double));
		ebex_genetic_copy_data(m_pop, m_daughter, m_dad, m_chromosome);
		ebex_genetic_copy_data(m_pop, m_son, m_mom, m_chromosome);
	}

	return;
}

static void ebex_genetic_mating_double_mean(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome)
{
	size_t j; /* Loop over all alleles. */

	/* Checks. */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed.");

	for (j = 0; j < m_dad->chromosome[m_chromosome].length; j++)
	{
		m_son->chromosome[m_chromosome].dbl_allele[j] = 0.5 * (m_dad->chromosome[m_chromosome].dbl_allele[j] + m_mom->chromosome[m_chromosome].dbl_allele[j]);
		m_daughter->chromosome[m_chromosome].dbl_allele[j] = 0.5 * (m_dad->chromosome[m_chromosome].dbl_allele[j] + m_mom->chromosome[m_chromosome].dbl_allele[j]);
	}

	return;
}


static void ebex_genetic_mating_double_allele_mixing(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome)
{
	size_t j; /* Loop over all alleles. */

	/* Checks. */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed.");

	for (j = 0; j < m_dad->chromosome[m_chromosome].length; j++)
	{
		if (ran_bool())
		{
			m_son->chromosome[m_chromosome].dbl_allele[j] = m_dad->chromosome[m_chromosome].dbl_allele[j];
			m_daughter->chromosome[m_chromosome].dbl_allele[j] = m_mom->chromosome[m_chromosome].dbl_allele[j];
		}
		else
		{
			m_daughter->chromosome[m_chromosome].dbl_allele[j] = m_dad->chromosome[m_chromosome].dbl_allele[j];
			m_son->chromosome[m_chromosome].dbl_allele[j] = m_mom->chromosome[m_chromosome].dbl_allele[j];
		}
	}

	return;
}

static void singlepoint_mating_double_chromosome(population_t *m_pop, double *m_dad, double *m_mom, double *m_son,
		double *m_daughter, size_t m_chromosomelen)
{
	int location; /* Point of mating */

	/* Checks */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to chromosome structure passed.");

	/* Choose mating point and perform operation */
	location = ran_uint_uniform(m_chromosomelen);

	memcpy(m_son, m_mom, location * sizeof(double));
	memcpy(m_daughter, m_dad, location * sizeof(double));

	memcpy(&m_son[location], &m_dad[location], (m_chromosomelen - location) * sizeof(double));
	memcpy(&m_daughter[location], &m_mom[location], (m_chromosomelen - location) * sizeof(double));

	return;
}


static void doublepoint_mating_double_chromosome(population_t *m_pop, double *m_dad, double *m_mom, double *m_son,
		double *m_daughter, size_t m_chromosomelen)
{
	int location1, location2; /* Points of mating. */

	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to chromosome structure passed.");

	/* Choose mating point and perform operation */
	location1 = ran_uint_uniform(m_chromosomelen);
	do
	{
		location2 = ran_uint_uniform(m_chromosomelen);
	} while (location2 == location1);

	if (location1 > location2)
	{
		EBEX_SWAP(location1, location2);
	}

	memcpy(m_son, m_dad, location1 * sizeof(double));
	memcpy(m_daughter, m_mom, location1 * sizeof(double));

	memcpy(&m_son[location1], &m_mom[location1], (location2 - location1) * sizeof(double));
	memcpy(&m_daughter[location1], &m_dad[location1], (location2 - location1) * sizeof(double));

	memcpy(&m_son[location2], &m_dad[location2], (m_chromosomelen - location2) * sizeof(double));
	memcpy(&m_daughter[location2], &m_mom[location2], (m_chromosomelen - location2) * sizeof(double));

	return;
}

static void ebex_genetic_mating_quaternion_mixing(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome)
{

	/* Checks */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed");

	if (ran_bool())
	{
		ebex_genetic_copy_chromosome(m_pop, m_son, m_dad, m_chromosome);
		ebex_genetic_copy_chromosome(m_pop, m_daughter, m_mom, m_chromosome);
	}
	else
	{
		ebex_genetic_copy_chromosome(m_pop, m_daughter, m_dad, m_chromosome);
		ebex_genetic_copy_chromosome(m_pop, m_son, m_mom, m_chromosome);
	}

	return;
}

static void ebex_genetic_mating_quaternion_mean(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome)
{
	size_t j; /* Loop over all alleles. */

	/* Checks. */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed.");

	for (j = 0; j < m_dad->chromosome[m_chromosome].length; j++)
	{
		ebex_quat_slerp(&m_son->chromosome[m_chromosome].q_allele[j], 0.5, &m_dad->chromosome[m_chromosome].q_allele[j], &m_mom->chromosome[m_chromosome].q_allele[j]);
		ebex_quat_slerp(&m_daughter->chromosome[m_chromosome].q_allele[j], 0.5, &m_dad->chromosome[m_chromosome].q_allele[j], &m_mom->chromosome[m_chromosome].q_allele[j]);
	}

	return;
}


static void ebex_genetic_mating_quaternion_allele_mixing(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter, size_t m_chromosome)
{
	size_t j; /* Loop over all alleles. */

	/* Checks. */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed.");

	for (j = 0; j < m_dad->chromosome[m_chromosome].length; j++)
	{
		if (ran_bool())
		{
			ebex_quat_copy(&m_son->chromosome[m_chromosome].q_allele[j], &m_dad->chromosome[m_chromosome].q_allele[j]);
			ebex_quat_copy(&m_daughter->chromosome[m_chromosome].q_allele[j], &m_mom->chromosome[m_chromosome].q_allele[j]);
		}
		else
		{
			ebex_quat_copy(&m_son->chromosome[m_chromosome].q_allele[j], &m_mom->chromosome[m_chromosome].q_allele[j]);
			ebex_quat_copy(&m_daughter->chromosome[m_chromosome].q_allele[j], &m_dad->chromosome[m_chromosome].q_allele[j]);
		}
	}

	return;
}


static void singlepoint_mating_quaternion_chromosome(population_t *m_pop, ebex_quat_t *m_dad, ebex_quat_t *m_mom, ebex_quat_t *m_son,
		ebex_quat_t *m_daughter, size_t m_chromosomelen)
{
	int location; /* Point of mating */

	/* Checks */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to chromosome structure passed.");

	/* Choose mating point and perform operation */
	location = ran_uint_uniform(m_chromosomelen);

	memcpy(m_son, m_mom, location * sizeof(ebex_quat_t));
	memcpy(m_daughter, m_dad, location * sizeof(ebex_quat_t));

	memcpy(&(m_son[location]), &(m_dad[location]), (m_chromosomelen - location) * sizeof(ebex_quat_t));
	memcpy(&(m_daughter[location]), &(m_mom[location]), (m_chromosomelen - location) * sizeof(ebex_quat_t));

	return;
}

static void doublepoint_mating_quaternion_chromosome(population_t *m_pop, ebex_quat_t *m_dad, ebex_quat_t *m_mom, ebex_quat_t *m_son,
		ebex_quat_t *m_daughter, size_t m_chromosomelen)
{
	int location1, location2; /* Points of mating. */

	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to chromosome structure passed.");

	/* Choose mating point and perform operation */
	location1 = ran_uint_uniform(m_chromosomelen);
	do
	{
		location2 = ran_uint_uniform(m_chromosomelen);
	} while (location2 == location1);

	if (location1 > location2)
	{
		EBEX_SWAP(location1, location2);
	}

	memcpy(m_son, m_dad, location1 * sizeof(ebex_quat_t));
	memcpy(m_daughter, m_mom, location1 * sizeof(ebex_quat_t));

	memcpy(&(m_son[location1]), &(m_mom[location1]), (location2 - location1) * sizeof(ebex_quat_t));
	memcpy(&(m_daughter[location1]), &(m_dad[location1]), (location2 - location1) * sizeof(ebex_quat_t));

	memcpy(&(m_son[location2]), &(m_dad[location2]), (m_chromosomelen - location2) * sizeof(ebex_quat_t));
	memcpy(&(m_daughter[location2]), &(m_mom[location2]), (m_chromosomelen - location2) * sizeof(ebex_quat_t));

	return;
}

static inline void ebex_genetic_mating_singlepoints(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter)
{
	size_t i; /* Loop variable over all chromosomes */

	/* Checks */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed");

	for (i = 0; i < m_pop->num_chromosomes; i++)
	{
		ebex_genetic_copy_data(m_pop, m_son, m_dad, i);
		ebex_genetic_copy_data(m_pop, m_daughter, m_mom, i);
		switch (m_dad->chromosome[i].type)
		{
			case CHROMOSOME_TYPE_DOUBLE:
				singlepoint_mating_double_chromosome(m_pop, m_dad->chromosome[i].dbl_allele,
								m_mom->chromosome[i].dbl_allele, m_son->chromosome[i].dbl_allele,
								m_daughter->chromosome[i].dbl_allele, m_dad->chromosome[i].length);
				break;
			case CHROMOSOME_TYPE_QUATERNION:
				singlepoint_mating_quaternion_chromosome(m_pop, m_dad->chromosome[i].q_allele,
						m_mom->chromosome[i].q_allele, m_son->chromosome[i].q_allele,
						m_daughter->chromosome[i].q_allele, m_dad->chromosome[i].length);
				break;
			default:
				dief("Invalid Chromosome type '%d'!", (int)m_dad->chromosome[i].type);
				break;
		}

	}

	return;
}

static inline void ebex_genetic_mating_doublepoints(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter)
{
	size_t i; /* Loop variable over all chromosomes */

	/* Checks */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed");

	for (i = 0; i < m_pop->num_chromosomes; i++)
	{
		ebex_genetic_copy_data(m_pop, m_son, m_dad, i);
		ebex_genetic_copy_data(m_pop, m_daughter, m_mom, i);

		switch (m_dad->chromosome[i].type)
		{
			case CHROMOSOME_TYPE_DOUBLE:
				doublepoint_mating_double_chromosome(m_pop, m_dad->chromosome[i].dbl_allele,
						m_mom->chromosome[i].dbl_allele, m_son->chromosome[i].dbl_allele,
						m_daughter->chromosome[i].dbl_allele, m_dad->chromosome[i].length);
				break;
			case CHROMOSOME_TYPE_QUATERNION:
				doublepoint_mating_quaternion_chromosome(m_pop, m_dad->chromosome[i].q_allele,
						m_mom->chromosome[i].q_allele, m_son->chromosome[i].q_allele,
						m_daughter->chromosome[i].q_allele, m_dad->chromosome[i].length);
				break;
			default:
				dief("Invalid Chromosome type '%d'!", (int)m_dad->chromosome[i].type);
				break;
		}
	}

	return;
}

static inline void ebex_genetic_mating_mixing(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter)
{
	size_t i; /* Loop variable over all chromosomes */

	/* Checks */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed");

	for (i = 0; i < m_pop->num_chromosomes; i++)
	{
		ebex_genetic_copy_data(m_pop, m_son, m_dad, i);
		ebex_genetic_copy_data(m_pop, m_daughter, m_mom, i);
		switch (m_dad->chromosome[i].type)
		{
			case CHROMOSOME_TYPE_DOUBLE:
				ebex_genetic_mating_double_mixing(m_pop, m_dad, m_mom, m_son, m_daughter, i);
				break;
			case CHROMOSOME_TYPE_QUATERNION:
				ebex_genetic_mating_quaternion_mixing(m_pop, m_dad, m_mom, m_son, m_daughter, i);
				break;
			default:
				dief("Invalid Chromosome type '%d'!", (int)m_dad->chromosome[i].type);
				break;
		}
	}

	return;
}

static inline void ebex_genetic_mating_mean(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter)
{
	size_t i; /* Loop variable over all chromosomes */

	/* Checks */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed");

	for (i = 0; i < m_pop->num_chromosomes; i++)
	{
		ebex_genetic_copy_data(m_pop, m_son, m_dad, i);
		ebex_genetic_copy_data(m_pop, m_daughter, m_mom, i);
		switch (m_dad->chromosome[i].type)
		{
			case CHROMOSOME_TYPE_DOUBLE:
				ebex_genetic_mating_double_mean(m_pop, m_dad, m_mom, m_son, m_daughter, i);
				break;
			case CHROMOSOME_TYPE_QUATERNION:
				ebex_genetic_mating_quaternion_mean(m_pop, m_dad, m_mom, m_son, m_daughter, i);
				break;
			default:
				dief("Invalid Chromosome type '%d'!", (int)m_dad->chromosome[i].type);
				break;
		}
	}

	return;
}

static inline void ebex_genetic_mating_allele_mixing(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son,
		organism_t *m_daughter)
{
	size_t i; /* Loop variable over all chromosomes */

	/* Checks */
	if (!m_dad || !m_mom || !m_son || !m_daughter)
		die("Null pointer to entity structure passed");

	for (i = 0; i < m_pop->num_chromosomes; i++)
	{
		ebex_genetic_copy_data(m_pop, m_son, m_dad, i);
		ebex_genetic_copy_data(m_pop, m_daughter, m_mom, i);
		switch (m_dad->chromosome[i].type)
		{
			case CHROMOSOME_TYPE_DOUBLE:
				ebex_genetic_mating_double_allele_mixing(m_pop, m_dad, m_mom, m_son, m_daughter, i);
				break;
			case CHROMOSOME_TYPE_QUATERNION:
				ebex_genetic_mating_quaternion_allele_mixing(m_pop, m_dad, m_mom, m_son, m_daughter, i);
				break;
			default:
				dief("Invalid Chromosome type '%d'!", (int)m_dad->chromosome[i].type);
				break;
		}
	}

	return;
}

void ebex_genetic_mate(population_t *m_pop, organism_t *m_dad, organism_t *m_mom, organism_t *m_son, organism_t *m_daughter)
{

	switch(m_pop->mating_type)
	{
		case genetic_mating_type_singlepoint:
			ebex_genetic_mating_singlepoints(m_pop, m_dad, m_mom, m_son, m_daughter);
			break;
		case genetic_mating_type_doublepoint:
			ebex_genetic_mating_doublepoints(m_pop, m_dad, m_mom, m_son, m_daughter);
			break;
		case genetic_mating_type_mixing:
			ebex_genetic_mating_mixing(m_pop, m_dad, m_mom, m_son, m_daughter);
			break;
		case genetic_mating_type_allele_mixing:
			ebex_genetic_mating_allele_mixing(m_pop, m_dad, m_mom, m_son, m_daughter);
			break;
		case genetic_mating_type_mean:
			ebex_genetic_mating_mean(m_pop, m_dad, m_mom, m_son, m_daughter);
			break;
		case genetic_mating_type_none:
			break;
	}
}
